
void EncodeSteeringFrame(unsigned char *frame, double steering_angle, unsigned char &heartbeat)
{
    heartbeat++;
    steering_angle = steering_angle * 0.1 + 15750;
    auto angle_raw = static_cast<unsigned short>(steering_angle);
    frame[0] = angle_raw & 0xff;
    frame[1] = angle_raw >> 8;
    frame[2] = 0x31; // enter
    frame[3] = 0x00;
    frame[4] = 0x54; // angularspeed
    frame[5] = 0x00;
    frame[6] = 0x00;
    frame[7] = heartbeat;
}

void EncodeBrakeFrame(unsigned char *frame, double brake, unsigned char &heartbeat)
{
    heartbeat++;
    frame[0] = 0x00; // unbark
    frame[1] = static_cast<unsigned char>(brake);
    frame[2] = 0x00;
    frame[3] = 0x00;
    frame[4] = 0x00;
    frame[5] = 0x00;
    frame[6] = 0x00;
    frame[7] = heartbeat;
}

struct MotorControlCfg
{
    enum Mode
    {
        StandBy = 0b00,
        TorqueControl = 0b01,
        VelocityControl = 0b10,
        Discharge = 0b11,
    };
    bool m1_enable;
    Mode m1_mode;
    bool m1_rotation;
    double m1_value;
    double m1_speed_limit;
    bool m2_enable;
    Mode m2_mode;
    bool m2_rotation;
    double m2_value;
    double m2_speed_limit;
    double power_limit;
    unsigned char hb;

    void Init()
    {
        m1_enable = true;
        m1_mode = Mode::VelocityControl;
        m1_rotation = true;
        m1_value = 0;
        m1_speed_limit = 1000;
        m2_enable = true;
        m2_mode = Mode::VelocityControl;
        m2_rotation = true;
        m2_value = 0;
        m2_speed_limit = 1000;
        power_limit = 1000; // max1000
        hb = 0;
    }

    void Encode2MotorFrame(unsigned char *frame)
    {

        unsigned int value_m1_raw = 0;
        unsigned int value_m2_raw = 0;
        if (m1_mode == Mode::TorqueControl)
        {
            value_m1_raw = m1_value + 2000;
        }
        else if (m1_mode == Mode::VelocityControl)
        {
            value_m1_raw = (m1_value + 10000) / 5;
        }
        else
        {
            value_m1_raw = 0;
        }
        if (m2_mode == Mode::TorqueControl)
        {
            value_m2_raw = m2_value + 2000;
        }
        else if (m2_mode == Mode::VelocityControl)
        {
            value_m2_raw = (m2_value + 10000) / 5;
        }
        else
        {
            value_m2_raw = 0;
        }
        unsigned int limit_m1_raw = m1_speed_limit / 15;
        unsigned int limit_m2_raw = m2_speed_limit / 15;
        unsigned int power_limit_raw = power_limit;
        hb++;
        if (hb > 15)
            hb = 0;
        frame[0] = m1_enable ? 0b1 : 0b0;
        frame[0] |= m1_mode << 1;
        frame[0] |= (m1_rotation ? 0b1 : 0b0) << 3;
        frame[0] |= ((value_m1_raw & 0xf) << 4);
        frame[1] = (value_m1_raw >> 4) & 0xff;
        frame[2] = limit_m1_raw & 0xff;
        frame[3] = (limit_m1_raw >> 8) & 0b1;

        frame[3] |= (m2_enable ? 0b1 : 0b0) << 1;
        frame[3] |= m2_mode << 2;
        frame[3] |= (m2_rotation ? 0b1 : 0b0) << 4;
        frame[3] |= ((value_m2_raw & 0b111) << 5);
        frame[4] = (value_m2_raw >> 3) & 0xff;
        frame[5] = value_m2_raw >> 11 & 0b1;
        frame[5] |= (limit_m2_raw & 0b1111111) << 1;
        frame[6] = (limit_m2_raw >> 7) & 0b11;

        frame[6] |= (power_limit_raw & 0b11, 1111) << 2;
        frame[7] = (power_limit_raw >> 6) & 0xf;
        frame[7] |= (hb & 0xf) << 4;
        for (int i = 0; i < 8; ++i)
        {
            std::cout << std::hex << std::setw(2) << std::setfill('0') << (int)frame[i] << " ";
        }
        std::cout << std::dec << std::endl;
    }
};

void DecodeSpeed(const unsigned char *const steering_frame, double &speed)
{
    unsigned int speed_raw = 0;
    speed_raw = speed_raw | (steering_frame[6] & 0b11);
    speed_raw << 8;
    speed_raw = speed_raw | steering_frame[5];
    speed_raw << 6;
    speed_raw = speed_raw | (steering_frame[4] >> 2);
    speed = speed_raw;
    speed = speed * 5 - 10000;
}